#ifndef FORMATION_CTRL_NODE_H  // 防止重复包含
#define FORMATION_CTRL_NODE_H

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <iostream>
#include <map>
#include <math.h>
#include <string>
#include <vector>

using namespace std;
class FormationCtrlNode
{
public:
    FormationCtrlNode();
    ~FormationCtrlNode();
    int get_thread_numbers();
private:
    ros::NodeHandle _nh;
    ros::Subscriber _sub_leader_odom, _sub_other_robot_odom, _sub_robot_odom, _sub_cmd;
    ros::Publisher _pub_formation_cmd, _pub_show_formation_target;
    ros::Timer _timer1;

    nav_msgs::Odometry _leader_odom, _other_robot_odom, _this_robot_odom;
    geometry_msgs::Twist _sub_twist_cmd;
    geometry_msgs::Point _pub_formation_target;
    geometry_msgs::PointStamped _show_formation_target;

    // 当前跟随机器人的命名空间
    string _follower_frame;
    string _simulation;

    double _cmd_distance, _cmd_angle;   // control parameters
    double _initial_len,_initial_angle;
    double _dt;
    int _controller_freq, _thread_numbers;

    void leader_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void other_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void this_robot_odom_CB(const nav_msgs::Odometry::ConstPtr& odomMsg);
    void read_cmd(const geometry_msgs::Twist::ConstPtr& cmd);  // 读取用户输入的命令

    void controlLoopCB(const ros::TimerEvent&);  // 控制回调函数
};



#endif // FORMATION_CTRL_NODE_H